#!/usr/bin/env python3
import os
import cv2
import time
import threading
import numpy as np
from queue import Queue
from camera.imx219_cam import Imx219Camera
from camera.tf25a_ir import Tf25aCamera

os.environ["DISPLAY"] = ":0"

class CameraThread(threading.Thread):
    def __init__(self, camera, frame_queue, name):
        super().__init__(daemon=True)
        self.camera = camera
        self.frame_queue = frame_queue
        self.running = False
        self.name = name

    def run(self):
        self.running = True
        print(f"[THREAD] 摄像头线程 {self.name} 启动")
        while self.running:
            frame = self.camera.read()
            if frame is not None:
                if self.frame_queue.full():
                    self.frame_queue.get_nowait()
                self.frame_queue.put(frame)
            time.sleep(0.01)

    def stop(self):
        self.running = False
        print(f"[THREAD] 摄像头线程 {self.name} 停止")

def main():
    cam0 = Imx219Camera("/dev/video22", name="IMX219-0")
    cam1 = Imx219Camera("/dev/video31", name="IMX219-1")
    ir_cam = Tf25aCamera("/dev/video40", name="TF25A")

    cam0.open_v4l2()
    cam1.open_v4l2()
    ir_cam.open()

    q_cam0 = Queue(maxsize=2)
    q_cam1 = Queue(maxsize=2)
    q_ir   = Queue(maxsize=2)

    t_cam0 = CameraThread(cam0, q_cam0, "IMX219-0")
    t_cam1 = CameraThread(cam1, q_cam1, "IMX219-1")
    t_ir   = CameraThread(ir_cam, q_ir, "TF25A")

    t_cam0.start()
    t_cam1.start()
    t_ir.start()

    time.sleep(1)

    print("[INFO] 开始采集，按 'q' 退出或 Ctrl+C 停止")
    cv2.namedWindow("Camera View", cv2.WND_PROP_FULLSCREEN)
    cv2.setWindowProperty("Camera View", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)

    try:
        while True:

            if not q_cam0.empty():
                frame0 = q_cam0.get_nowait()
                frame0_resized = cv2.resize(frame0, (960, 540))

            if not q_cam1.empty():
                frame1 = q_cam1.get_nowait()
                frame1_resized = cv2.resize(frame1, (960, 540))

            if not q_ir.empty():
                frame_ir = q_ir.get_nowait()
                frame_ir_resized = cv2.resize(frame_ir, (960, 500))

            top_row = np.hstack((frame0_resized, frame1_resized))
            bottom_row = np.hstack((frame_ir_resized, frame_ir_resized)) 

            full_frame = np.vstack((top_row, bottom_row))  


            cv2.imshow("Camera View", full_frame)

            key = cv2.waitKey(1) & 0xFF
            if key == ord('q'):
                print("[INFO] 用户按下 'q'，停止采集")
                break

    except KeyboardInterrupt:
        print("\n[INFO] 用户中断，停止采集")
    except Exception as e:
        print(f"[ERROR] 发生错误: {e}")

    finally:
        print("\n[MAIN] 正在停止所有线程...")
        t_cam0.stop()
        t_cam1.stop()
        t_ir.stop()

        cam0.release()
        cam1.release()
        ir_cam.release()

        cv2.destroyAllWindows()
        print("[MAIN] 退出完成")


if __name__ == "__main__":
    main()
